/*
 * Authors:   Krzytom
 * 
 */


includes KrzytomMsg;
includes KrzytomConst;
includes Point;

/**
 * ustalanie pozycji 
 * tymczasowo wezly o id 0,1,2 uznawane sa jako latarnie
 * i maja wpisane na sztywno pozycje
 * pozostale wezly nie znaja pozycji i staraja sie ja ustalic
 *
 */
module KrzytomM
{
  provides interface StdControl;
  uses {
    interface Timer;
    interface Leds;
    interface StdControl as CommControl;
    interface SendMsg;
    interface ReceiveMsg;
    interface StdControl as UARTCommControl;
    interface SendMsg as UARTSend;
/*#ifdef PLATFORM_PC
	interface CC1000Control;
#else
	interface CC2420Control;
#endif*/
  }
}
implementation
{
  TOS_Msg msg;
  TOS_Msg msg_uart;
  int16_t power;
  bool pending;
  float pos_tab[MAX_NEIGHBOURS][5]; // tablica: id sasiada, posX, posY, e, odleglosc
  uint8_t ncount = 0; // licznik sasiadow dla powyzszej tab
  // nasza pozycja i blad:
  float x = 0;
  float y = 0;
  float e = 0.5;
  // rodzaj wezla (kotwica/latarnia/statek)
  uint8_t node_type = SHIP;


  /**
   * odleglosc miedzy 2 punktami
   */
  float p_dist(struct Point p1, struct Point p2) {
	  return sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y));
  }

  /*
   * znalezienie jednego z punktow w,t,f
   */
  struct Point findWTF(uint8_t a1, uint8_t a2, uint8_t a3) {
	struct Point p1 = {pos_tab[a1][1], pos_tab[a1][2]};
	float r1 = pos_tab[a1][4];
	struct Point p2 = {pos_tab[a2][1], pos_tab[a2][2]};
	float r2 = pos_tab[a2][4];
	struct Point p3 = {pos_tab[a3][1], pos_tab[a3][2]};
	float d, d1, h, x3, y3;
	struct Point p4_i, p4_ii;
	struct Point p_selected;
	float diff;

	d = p_dist(p1, p2); // odleglosc miedzy srodkami

	while (d > r1 + r2) {
		dbg(DBG_USR1, "WTF okregi sie nie przecinaja, zwiekszam promienie\n");
		diff = d - r1 - r2;
		r1 += diff/2.0 + 0.001;
		r2 += diff/2.0 + 0.001;
	}

	d1 = (r1*r1 - r2*r2 + d*d)/(2*d);
	h = sqrt(r1*r1 - d1*d1);
	
	x3 = p1.x + (d1 * (p2.x - p1.x)) / d;
	y3 = p1.y + (d1 * (p2.y - p1.y)) / d;

	p4_i.x = x3 + (h * (p2.y - p1.y)) / d;
	p4_i.y = y3 - (h * (p2.x - p1.x)) / d;
	p4_ii.x = x3 - (h * (p2.y - p1.y)) / d;
	p4_ii.y = y3 + (h * (p2.x - p1.x)) / d;

	dbg(DBG_USR1, "WTF Srodki okregow: \n\tp1.x=%.2f, p1.y=%.2f\n\tp2.x=%.2f, p2.y=%.2f\n", p1.x, p1.y, p2.x, p2.y);
	dbg(DBG_USR1, "WTF Punkty przeciecia: \n\tx1=%.2f, y1=%.2f\n\tx2=%.2f, y2=%.2f\n", p4_i.x, p4_i.y, p4_ii.x, p4_ii.y);

	p_selected = p_dist(p4_i, p3) < p_dist(p4_ii, p3) ? p4_i : p4_ii;

	dbg(DBG_USR1, "WTF Zwyciezca: \n\tx=%.2f, y=%.2f\n", p_selected.x, p_selected.y);

	return p_selected;
  }

  bool find_points(uint8_t* anchors); 
  /**
   * aktualizacja naszej pozycji
   */
  bool update_pos() {
	  uint8_t anchors[3];
	  struct Point w, t, f;

	  if (find_points(anchors)) {
		  w = findWTF(anchors[0], anchors[1], anchors[2]);
		  t = findWTF(anchors[0], anchors[2], anchors[1]);
		  f = findWTF(anchors[1], anchors[2], anchors[0]);
		  x = (w.x + t.x + f.x) / 3;
		  y = (w.y + t.y + f.y) / 3;
		  e = 0x0;
		  dbg(DBG_USR1, "Aktualizacja pozycji: \n\tx=%.2f, \n\ty=%.2f, \n\te=%.2f\n", x, y, e);
		  return TRUE;
	  }
	  return FALSE;
  }

  int16_t power2dbm(uint8_t tpower) {
	  switch (tpower) {
		  case 3 : return -25;
		  case 7 : return -15;
		  case 11 : return -10;
		  case 15 : return -7;
		  case 19 : return -5;
		  case 23 : return -3;
		  case 27 : return -1;
		  case 31 : return 0; 
		  default : return 0;
	  }

  }

  int16_t rssi2dbm(uint16_t rssi) {
	  int16_t irssi = (int16_t) rssi;
	  return irssi - 45;
  }
  float db2w(int16_t arg) {
	  float w;
	  w = pow(10,(float)arg/10.0);
	  return w;
  }
	
  /**
   * funkcja obliczajaca odleglosc od sasiada
   */
  float dist(uint16_t rssi, int16_t tpower) {
	  float d;
	  int16_t diff_dbm;
	  int16_t rssi_dbm;
	  int16_t power_dbm;
	  rssi_dbm = rssi2dbm(rssi);
	  power_dbm = (int16_t)tpower;

	  diff_dbm = power_dbm - rssi_dbm;
	  d = sqrt(sqrt(db2w(rssi_dbm)/db2w(power_dbm)));
	  dbg(DBG_USR1, "Dist = %.2f\n", d);
	  return d;
  }

  /**
   * funkcja szuka 3 punktow odniesienia
   * anchors - wskaznik na tablice do wypelnienia 3 indeksami tablicy sasiadow
   * nie sprawdzane jest czy sa conajmniej 3 punkty w tablicy 
   * ==> wiec wywolywac tylko jak mamy przynajmniej 3
   */
  bool find_points(uint8_t* anchors) {
	uint8_t i, j;
	float ee;

	for (j=0; j<3; j++) {
		ee = 9000;
		for (i=0; i<ncount; i++) {
			if (pos_tab[i][3] < ee && (j < 1 || i != anchors[0]) && (j < 2 || i != anchors[1])) { 
				anchors[j] = i;
				ee = pos_tab[i][3];
			}
		}
	}
	dbg(DBG_USR1, "Indeksy wybranych kotwic: %d, %d, %d\n", anchors[0], anchors[1], anchors[2]);
	return TRUE;
  }

  /**
   * funkcja dodaje pozycje sasiada o danym id do tablicy
   * (sprawdza czy taki juz jest i czy ta pozycja jest lepsza itp)
   */
  bool pos_add(float id, float posX, float posY, float ee, float distance) {
	  int i;

	  for (i=0; i< ncount; i++) {
		  if (pos_tab[i][0] == id) {
			  if (pos_tab[i][3] > ee) {
				  pos_tab[i][1] = posX;
				  pos_tab[i][2] = posY;
				  pos_tab[i][3] = ee;
				  pos_tab[i][4] = distance;
				  return TRUE;
			  }
			  return FALSE;
		  }
	  }

	  dbg(DBG_USR1, "Dodaje kotwice: id=%d, posX=%.2f, posY=%.2f, e=%.2f\n", (int)id, posX, posY, ee);
	  // nie bylo go w tablicy:
	  pos_tab[ncount][0] = id; // !! brak sprawdzenia czy ncount nie przekracza maksimum!!
	  pos_tab[ncount][1] = posX;
	  pos_tab[ncount][2] = posY;
	  pos_tab[ncount][3] = ee;
	  pos_tab[ncount][4] = distance;
	  ncount++;
	  
	  return TRUE;
  }


  /**
   * Used to initialize this component.
   */
  command result_t StdControl.init() {
    call Leds.init();
    call Leds.yellowOff(); call Leds.redOff(); call Leds.greenOff();

    call CommControl.init();
    
    dbg(DBG_USR1, "Krzytom zainicjalizowany\n");
/*#ifdef PLATFORM_PC
	power = power2dbm(call CC1000Control.GetRFPower());
    dbg(DBG_USR1, "Radio power: %d\n", power);
#else
	power = power2dbm(call CC2420Control.GetRFPower());
    dbg(DBG_USR1, "Radio power: %d\n", power);
#endif*/

	//srand(time(0));

    return SUCCESS;
  }

  /**
   * Starts the SensorControl and CommControl components.
   * @return Always returns SUCCESS.
   */
  command result_t StdControl.start() {
	if (TOS_LOCAL_ADDRESS < 3) {
		node_type = BEACON; // dla nodow o id 0,1,2 ustawiamy ze sa latarniami
    	call Timer.start(TIMER_ONE_SHOT, (TOS_LOCAL_ADDRESS +1) * 12000);
	}
	if (node_type == BEACON) {
		call Leds.redOn();
	}

	// KOD TYMCZASOWY -- SZTYWNE WPISANIE POZYCJI WEZLOW 0,1,2:
		if (TOS_LOCAL_ADDRESS == 0) {
			x = 0.00;	y = 4.0;
		} else if (TOS_LOCAL_ADDRESS == 1) {
			x = 4.0;	y = 0.0;
		} else if (TOS_LOCAL_ADDRESS == 2) {
			x = 0.0; y = -4.0;
		}
	//////////////////////////////////

	call CommControl.start();
    return SUCCESS;
  }

  /**
   * Stops the SensorControl and CommControl components.
   * @return Always returns SUCCESS.
   */
  command result_t StdControl.stop() {
	if (TOS_LOCAL_ADDRESS < 3) {
		call Timer.stop();
	}
	call CommControl.stop();
    return SUCCESS;
  }

  task void uartTask() {
		call UARTSend.send(TOS_UART_ADDR, sizeof(struct KrzytomMsg), &msg_uart);
  }

  task void dataTask() {
    struct KrzytomMsg *pack;

	if (!pending){
		pending = TRUE;

		atomic {
		  pack = (struct KrzytomMsg *)msg.data;
		}

		pack->sourceMoteID = TOS_LOCAL_ADDRESS;
		pack->posX = x;
		pack->posY = y;
		pack->e = e;
		pack->power = power;

		/* Try to send the packet. Note that this will return
		 * failure immediately if the packet could not be queued for
		 * transmission.
		 */
		if (call SendMsg.send(TOS_BCAST_ADDR, sizeof(struct KrzytomMsg), &msg)) {
	  		dbg(DBG_USR1, "Wysylam swoja pozycje: %.2f %.2f\n", x, y);
			msg_uart = msg;
			post uartTask();

			if (e < 1) {
				node_type = BEACON;
				call Leds.redOn();
			} else 
				node_type = ANCHOR;

			call Leds.greenOn();
			return;
		}
	}
  }
 
  /**
   * Signalled when the previous packet has been sent.
   * @return Always returns SUCCESS.
   */
  event result_t SendMsg.sendDone(TOS_MsgPtr sent, result_t success) {
	pending = FALSE;
    return SUCCESS;
  }

  event result_t UARTSend.sendDone(TOS_MsgPtr sent, result_t success) {
    return SUCCESS;
  }

  /**
   * Signalled when the clock ticks.
   * @return The result of calling ADC.getData().
   */
  event result_t Timer.fired() {
	post dataTask();
    return SUCCESS;
  }

  /**
   * Signalled when the reset message counter AM is received.
   * @return The free TOS_MsgPtr. 
   */
  event TOS_MsgPtr ReceiveMsg.receive(TOS_MsgPtr m) {
	struct KrzytomMsg *rec;


	if (node_type == BEACON) { // latarnia ignoruje te wiadomosci
		return m;
	}

	atomic {
		rec = (struct KrzytomMsg *)m->data;
	}

	
	dbg(DBG_USR1, "Odebralem taka wiadomosc: id %d, x=%.2f y=%.2f, e=%.2f, power=%d\n", rec->sourceMoteID, rec->posX, rec->posY, rec->e, rec->power);
	dbg(DBG_USR1, "RSSI = %d\n", (int16_t)m->strength);

	pos_add((float)(rec->sourceMoteID), rec->posX, rec->posY, rec->e, dist(m->strength, rec->power)); 

	call Leds.yellowToggle();

	if (ncount >= 3) { // mamy  conajmniej 3 sasiadow na liscie
		if (update_pos()) { // ustalono lub zaktualizowano pozycje
    		call Timer.start(TIMER_ONE_SHOT, TOS_LOCAL_ADDRESS*1000); // TMP
//			post dataTask();
		}
	}

	return m;
  }
}
